'''
Michael Cabot

To run this program you need to install the package OpenCV at:
http://opencv.willowgarage.com/wiki/InstallGuide
The documentation can be found at:
http://opencv.willowgarage.com/documentation/python/index.html

# PSEUDO
# 1. filter image for correctly colored part-of-lines
# 2. match part-of-lines to form blobs by assigning part-of-lines same ID 
# 3. form blobs and assign new ID's
# 4. filter for blobs that are too small
# 5. calculate the goal distance based on the blobs that remain


Setting the HSV values (KVW in dutch):
'Normal' programs (e.g. gimp) use different HSV ranges than openCV.
GIMP
H: [0 - 360]
S: [0 - 100]
V: [0 - 100]
openCV
H: [0 - 180]
S: [0 - 255]
V: [0 - 255]

Convert the HSV values taken from gimp:
H_opencv = H_gimp * 180/360 = H_gimp * 0.5
S_opencv = S_gimp * 255/100 = S_gmip * 2.55
V_opencv = V_gimp * 255/100 = V_gimp * 2.55

                                                                                                  ^_^


UPDATED (01-07-2011 by Michael)
I've removed all old update comments.
The blobfilter now works reasonably. There were weird result, because the blob-array wasn't updated.
Other problems occur (math domain error) when a nao is partially in front of one of the goal posts.
This makes it seem as if that post is at a greater distance than it really is.
@~1  (This doesn't work yet). If the nao is looking at the goal almost from the side, then only 1
goal post will be found. I think this is due to the angle of the cross bar that goes more vertical
when seen from aside. The line blobs of the bar are therefore seen as one of the posts and added to
that blob. So, the bloblines in the bar aren't as long when seen from the front and therefore added
to the blob of a post. I've tried to fix this by checking if the start and end of a blobline doesn't
differ to much from the average start en stop of the blob... but it doesn't work (yet). 
>Awesome idea of Eszter: don't make line blobs horizontally, but vertically :D.

UPDATED (02-07-2011 by Michael)
-I've reversed the way lineBlobs, that are below each other, get the same ID. Now looping bottom-up.
Because of this the two posts can be found when they first wouldn't be. Further testing needs to be
done to ensure that the goal posts always will be found.
-I've changed the minimal angle between the two posts from 0.15 to 0.10 radians, based on the fact
that the goal posts are correctly found when the angle between them is 0.114.
-I've changed the maxWidth from 55 to 1000. Since the width of the image is only 160 pixels (with
resolution 0) a maxWidth of 1000 means there is no maxWidth. By changing this the nao will find a
goal post if standing a few centimeters in front of it. I thing there needs to be a more sensible
maxWidth than 1000. If a blobline covers the whole width of the image there is no telling what the
real width of the goal post is (since de edges lie outside of the image).
'''

from PIL import Image
import math
import cv
import time

width = None                                    #the width of the picture (is set in 'markOrange')
height = None                                   #the height of the picture (is set in 'markOrange')



photo = None                          #photo taken by the Nao. Is set when in filter() or when consulting this program.
size = None

# PSEUDO
# 1. filter image for correctly colored part-of-lines
# 2. match part-of-lines to form blobs by assigning part-of-lines same ID 
# 3. form blobs and assign new ID's
# 4. filter for blobs that are too small
# 5. calculate the goal distance based on the blobs that remain
 

def run(im, color, head):
    now = time.time()
    #im = snapShot('python_GVM')[0]
    yawHead = head[1]
    im = convertImage(im)
    #cv.SaveImage('blob' + str(color)+ str(head[1]) +'Original.png',im)
    ori = im
    
    im = markGoalCV(im, color)
    #cv.SaveImage('test2.png',im)
    cv.Smooth(im, im, cv.CV_GAUSSIAN, 5,5)
    #cv.SaveImage('smoothed.png', im)
    image = im
    
    # find blobs 
    (width, height) = (160,120)
    blobID = 0
    data = {}
    threshold = 70   #threshold for the grayscale
    #threshold = 0
    
    '''
    Loop through the image with steps of 2 till you find a white pixel. If you
    find a white pixel for the first time, then loop 4 pixels back (why 4?) to
    find the exact location of the starting point. If (after finding a white
    pixels) you find a non-white pixel, then look 4 pixels back (again, why 4?)
    to find the exact ending point.
    
    reversed: returns an iterator that iterates through the list backwards.
    '''
    for y in reversed(range(height)):
    #for y in range(height):
        # Lijst van data waar grayscale waarde > 70
        data[y] = list()

        # Maak een dictionary voor de waardes die onder elkaar staan
        newLineBlob = dict()

        for x in range (4, width, 3):
            if( image[y,x] > threshold and not ('start' in newLineBlob)):
                # loop through part of image again to see if blob starts earlier.
                for x2 in range (1,4): 
                    if( image[y, x-x2] <= threshold ):
                        newLineBlob['start'] = x-x2+1
                        break

            elif image[y,x] <= threshold and 'start' in newLineBlob:
                for x2 in range (1,4): 
                    if image[y, x-x2] > threshold :
                        newLineBlob['end'] = x-x2
                        newLineBlob['ID'] = blobID
                        newLineBlob['used'] = False

                        newLineBlob['height'] = y
                        
                        data[y].append(newLineBlob)
                        # clear dict for new blobs on the same line = reinitialization
                        newLineBlob = dict()
                        blobID += 1
                        break
            
            elif x >= width-4 and 'start' in newLineBlob:
                # loop through image again until end is found
                for x2 in range (1,3):
                    # look for possible end-of-blobs
                    if image[y, x+x2] <= threshold or x+x2 == width-1 :
                        newLineBlob['end'] = x-x2
                        newLineBlob['ID'] = blobID
                        newLineBlob['used'] = False

                        newLineBlob['height'] = y
                        
                        data[y].append(newLineBlob)
                        
                        # clear dict for new blobs on the same line = reinitialization. Is it really needed? Not sure. Probably not.
                        newLineBlob = dict()
                        blobID += 1
                        break
                    
    #for y in range(len(data)):
     #   print 'data: ', data[y]

    maxWidth = 1000 #max width of a pole (if maxWidth>160, then there is no max, since the image width is 160)
    # give lineBlobs that are below each other the same ID
    for y in reversed(range(1, len(data))):
        for curdata in data[y]:
            for belowdata in data[y-1]:
                
                # check if blobs on lines above each other are close enough, using blobID to append lineblobs
                
                curStart = curdata['start']
                curEnd = curdata['end']
                belStart = belowdata['start']
                belEnd = belowdata['end']
                #distCurBel = math.fabs( (curEnd+curStart)/2.0 - (belEnd+belStart)/2.0 )# the distance between the centre of curdata and beldata
                if curEnd - curStart < maxWidth: 
                    #if curStart < belEnd and curEnd > belStart and distCurBel < (curEnd-curStart):
                    if curStart < belEnd and curEnd > belStart and \
                       math.fabs(curStart-belStart)<math.fabs(curStart-belEnd) and math.fabs(curEnd-belEnd)<math.fabs(curEnd-belStart):
                        if belowdata['used'] == False:
                            belowdata['ID'] = curdata['ID']
                            belowdata['used'] = True
                        else:
                            curdata['ID'] = belowdata['ID']
                            curdata['used'] = True

    #print '-------------------------------'
    #for y in range(len(data)):
     #   print 'data: ', data[y]

    # make blobs
    usedIDs = {}
    blobs = {}
    ID = 0
    for y in range(len(data)):
        for curdata in data[y]:
            curID = curdata['ID']
            start = curdata['start']
            end = curdata['end']
                
            # if a found ID is not yet in the found ID's
            if(not(curID in usedIDs)):
                # add ID plus a damn lot of info to the array
                blobs[ID] = {'minY':y, 'maxY':y,'minX': start, 'maxX': end,'totalWidth': end-start, 'height':1, 'array':[(start,end)], 'sumStart':start, 'sumEnd':end}
                usedIDs[curdata['ID']] = ID
                ID += 1
            else:
                # change info depending on current y and max and min x
                blob = blobs[usedIDs[curID]]
                
                if( y > blob['maxY'] ):
                    blob['maxY'] = y
                if( blob['minX'] > start ):
                    blob['minX'] = start
                if( blob['maxX'] < end ):
                    blob['maxX'] = end

                blob['array'].append(  (start, end) )           #don't comment this out!!
                blob['height'] += 1
                blob['totalWidth'] += end-start

                blob['sumStart'] += start
                blob['sumEnd'] += end

    #print '>>>>>>>>>>>>>>>>>>>>'
    #for i in range(len(blobs)):
     #   print blobs[i]


    # remove lineBlob outliers from the blob: line blobs with a width greater than the average width
    # and make a new blob
    for i in range(len(blobs)):
        blob = blobs[i]
        array = blob['array']
        averageWidth = blob['totalWidth']/blob['height']
        averageStart = blob['sumStart']/blob['height']
        averageEnd = blob['sumEnd']/blob['height']
        
        j = 0
        while j<len(array):
            (start, end) = array[j]
            # the width of a blobLine must be similar to the average width
            #~1 see comments at the start of the file
            #if end-start > averageWidth*1.2 and math.fabs(averageStart-start)>0.8*averageWidth and math.fabs(averageEnd-end)>0.8*averageWidth:
            if end-start > averageWidth*1.2:
                array.pop(j)
                blob['totalWidth'] -= end-start
                blob['height'] -= 1   
            else:
                j += 1

    #print '%%%%%%%%%%%%%%%%%%%%%%%'
    #for i in range(len(blobs)):
     #   print blobs[i]

    ID = 0
    filteredblobs = dict()
    for i in range(len(blobs)):
        blob = blobs[i]    
        # if a blob is too wide or too long, delete it 
        if blob['maxX'] - blob['minX'] < 4  or blob['height'] < 20:
            pass
        else:
            filteredblobs[ID] = blob
            ID += 1

    ''' 
    # draw on original image ori the blobs as rectangles.
    for i in range (len(filteredblobs)):
        blob = filteredblobs[i]
        array = blob['array']
        # find a blobArray almost at the bottom
        # '-1' if the array has length 1
        (minX, maxX) = array[ int(round( len(array)*0.8 ))-1 ]
        average = blob['totalWidth']/blob['height']
        blob['minX'] = minX
        blob['maxX'] = minX + average
        leftTop = (minX, blob['minY'])
        rightBot = (minX + average, blob['maxY'])
        cv.Rectangle(ori, leftTop, rightBot, cv.RGB(100, 100, 100))
    #name = 'Blob' + color + str(yawHead) + '.png'
    name = 'blob' + color + str(yawHead) + '.png'    
    cv.SaveImage(name, image)
    '''
    if( len(filteredblobs) >= 2 ):
        return (calcXangle(filteredblobs[0]['minX'], yawHead) + calcXangle(filteredblobs[1]['minX'], yawHead))  / 2.0
    elif len(filteredblobs) == 1:
        return calcXangle(filteredblobs[0]['minX'], yawHead)
        
def markGoalCV(im, color):
    # Size of the images
    hsvFrame = cv.CreateImage(size, cv.IPL_DEPTH_8U, 3)
    filter = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
    
    # Goalfilter values (made in Istanbul)
    if(color == 'blue'):
        hueMin = 116
        hueMax = 130
        saturationMin = 185 
        saturationMax = 255 
        valueMin = 75 
        valueMax = 210
    if(color == 'yellow'): #Should be all right
        hueMin = 20 
        hueMax = 41 
        saturationMin = 55 
        saturationMax = 255
        valueMin = 60
        valueMax = 210 
        
    hsvMin1 = cv.Scalar(hueMin, saturationMin, valueMin, 0)
    hsvMax1 = cv.Scalar(hueMax, saturationMax, valueMax, 0)
    cv.CvtColor(im, hsvFrame, cv.CV_BGR2HSV)
    cv.InRangeS(hsvFrame, hsvMin1, hsvMax1, filter)
    #cv.SaveImage('FILTERED' + color + '.png', filter)
    
    return filter

def convertImage(picture):
    global size
    size = (160, 120)
    # resize the image
    picture = picture.resize(size)
    # convert the type to OpenCV
    image = cv.CreateImageHeader(size, cv.IPL_DEPTH_8U, 3)
    cv.SetData(image, picture.tostring(), picture.size[0]*3)
    return image
    
def calcXangle(xcoord, yawHead):
	#print yawHead
	(width, height) = (160, 120)
	xDiff = width/2 - xcoord
	distanceToFrame = 0.5 * width / 0.42860054745600146
	xAngle = math.atan(xDiff/distanceToFrame)
	return xAngle + yawHead
